clear
%subplot(1,2,1);
obsHDL = plot(0, 0, 'd');
title('Raw Data');
xlabel('Longitude');
ylabel('Latitude');
axis([-6000 6000 0 9000]);
set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal

% subplot(1,2,2);
% filterHDL = plot(0, 0, 'd');
% title('Filter Output');

% axis([-600 600 0 650]);
% set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal

hold on
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

xlabel('Longitude');
ylabel('Latitude');

[B,A]=butter(1,.08,'low');

FC = 0;
FCT = 2;
CC = 0.9;
RC = 0;
RCT = 4;
R = 6371000;


%     c = urlread('http://192.168.1.102/obs');
fp = fopen('~/autoBoat/log/obsLogC');
c = fscanf(fp,'%s');
fclose(fp);
temp = strfind(c, '<vLon1>');
iterator = length(temp);
indexLon = zeros(181, iterator);
indexLonE = zeros(181, iterator);
indexLat = zeros(181, iterator);
indexLatE = zeros(181, iterator);

obsX = [];
obsY = [];
corrA = [];

for i = 1: 181
    tag = strcat('vLon', num2str(i-1));
    indexLon(i,:) = strfind(c, ['<' tag '>'])+length(['<' tag '>']);
    indexLonE(i,:) = strfind(c, ['</' tag '>']) -1;
    tag = strcat('vLat', num2str(i-1));
    indexLat(i,:) = strfind(c, ['<' tag '>'])+length(['<' tag '>']);
    indexLatE(i,:) = strfind(c, ['</' tag '>']) -1;
    
end


% find the stable referrence frame
for i = 1 : iterator
    for j = 1 : 181
        obsLon = str2double(c(indexLon(j,i):indexLonE(j,i)));
        obsLat = str2double(c(indexLat(j,i):indexLatE(j,i)));
        obsX = [obsX, obsLon];
        obsY = [obsY, obsLat];
    end
    if i ~= 1
         corrM = corrcoef(obsY, obsYL);
         if abs(corrM(2)) > CC
             disp('Similar!');
             FC = FC + 1;
             if FC > FCT
                 isCali = 0;
                 obsXR = obsX;
                 obsYR = obsY;
                 break;
             end
             
         else
             FC = 0;
         end
         obsXL = obsX;
         obsYL = obsY;
    else
        obsXL = obsX;
        obsYL = obsY;
    end
    obsX = [];
    obsY = [];
end


obsX = [];
obsY = [];

for i = i : iterator
    for j = 1 : 181
        obsLon = str2double(c(indexLon(j,i):indexLonE(j,i)));
        obsLat = str2double(c(indexLat(j,i):indexLatE(j,i)));
        obsX = [obsX, obsLon];
        obsY = [obsY, obsLat];
        
        
        
        %     padding = ones(1,20) * obsY(1);
        %     padding = [padding, obsY];
        %     filterY = filter(B,A,padding);
        %     set(filterHDL, 'XData', obsX, 'YData', filterY(21:end));
    end
    
    %correlation coefficient
    
    if i ~=1
        corrM = corrcoef(obsY, obsYR);
        if abs(corrM(2)) > CC
            
            
            % remove individual points
            
            counter = 1;
            
            for k = 1 : 180
                if abs(obsY(k+1) - obsY(k)) > 100
                    if counter < 10
                        if (k-counter+1) > 3
                            for m = k-counter+1 : k
                                
                                obsY(m) = (obsY(m-3)+obsY(m-2)+obsY(m-1))/3;
                                obsX(m) = (obsX(m-3)+obsX(m-2)+obsX(m-1))/3;
                            end
                        else
                            for m = k : -1: k-counter+1
                                obsY(m) = (obsY(m+3)+obsY(m+2)+obsY(m+1))/3;
                                obsX(m) = (obsX(m+3)+obsX(m+2)+obsX(m+1))/3;
                            end
                        end
                        
                    end
                    counter = 1;
                else
                    counter = counter + 1;
                end
            end
            
            if counter < 10
                for m = 182-counter : 181
                    obsY(m) = (obsY(m-3)+obsY(m-2)+obsY(m-1))/3;
                    obsX(m) = (obsX(m-3)+obsX(m-2)+obsX(m-1))/3;
                end
                
            end
            
             disp('refresh...');
             obsXL = obsX;
             obsYL = obsY;
             obsXR = obsX;
             obsYR = obsY;
        else
             disp('retain...');
             corrM = corrcoef(obsY, obsYL);
             if abs(corrM(2)) > CC
                 RC = RC + 1;
                 if RC > RCT
                     obsXR = obsX;
                     obsYR = obsY;
                 end
             end
             
             obsXL = obsX;
             obsYL = obsY;
        end
    else
        obsYL = obsY;
        obsXL = obsX;
    end
    

    set(obsHDL, 'XData', obsXR, 'YData', obsYR);
    drawnow
    pause(.2);
    obsX = [];
    obsY = [];
    
    
end
